#include <library.h>
using namespace boost;

static mt19937 rng(static_cast<unsigned> (std::time(0)));

double SampleNormal(double mean, double sigma) {
	/**
	 * 1. Create a Mersenne twister random number generator
	 * that is seeded once with #seconds since 1970
	 * 2. select Gaussian probability distribution
	 * 3. bind random number generator to distribution, forming a function
	 * 4. sample from the distribution
	 */
	normal_distribution<double> norm_dist(mean, sigma);
	variate_generator<mt19937&, normal_distribution<double> > normal_sampler(rng, norm_dist);

	return normal_sampler();
}

int Sample(VECTOR &weights) {
	double sum = 0;
	VECTOR space;
	for (int i = 0; i < weights.size(); i++) {
		sum += weights[i];
		space.push_back(sum);
	}
	boost::uniform_real<> dist(0, sum);
	boost::variate_generator<boost::mt19937&, boost::uniform_real<> > sample(rng, dist);

	return (std::lower_bound(space.begin(), space.end(), sample()) - space.begin());
	//	double randVal = RAND1 * sum;
	//	return BinarySearch(space,randVal);	This works too, but boost is way more efficient
}

void RouletteWheelSample(VECTOR &weights, std::vector<int> &pos) {
	double sum = 0, step, wt = 0;
	int N = weights.size();
	VECTOR space;
	for (int i = 0; i < N; i++) {
		sum += weights[i];
		space.push_back(sum);
	}
	step = sum / N;
	for (int i = 0, j = 0; i < N && wt <= sum; i++, wt += step) {
		pos.push_back(int(upper_bound (space.begin(), space.end(), wt) - space.begin()));
	}
	if (pos.size() != weights.size())
		ROS_ERROR("Undersampling %d %d", pos.size(), weights.size());
}

double SampleUniform(double min, double max) {
	boost::uniform_real<> uni_dist(min, max);
	boost::variate_generator<boost::mt19937&, boost::uniform_real<> > uni(rng, uni_dist);
	return uni();
}

int BinarySearch(VECTOR &array, double key) {
	/**
	 * Searches array for key.
	 * returns: index of the matching element if it finds key,
	 * or the index where it could be inserted+1;
	 */
	int first = 0;
	int last = array.size() - 1;

	while (first <= last) {
		int mid = (first + last) / 2;
		if (key > array[mid])
			first = mid + 1;
		else if (key < array[mid])
			last = mid - 1;
		else
			return mid;
	}
	return first; // failed to find key, this is where it should be inserted+1
}

double interpolate_at(double x, double y, double x1, double x2, double y1, double y2, double q11,
		double q12, double q21, double q22) {

	double dx = x2 - x1;
	double dy = y2 - y1;

	double r2 = ((x2 - x) * q11 + (x - x1) * q21) / dx;
	double r1 = ((x2 - x) * q12 + (x - x1) * q22) / dx;

	return ((y2 - y) * r1 + (y - y1) * r2) / dy;
}

double get_yaw(geometry_msgs::Quaternion qtr) {
	tf::Quaternion tqtr(qtr.x, qtr.y, qtr.z, qtr.w);
	return tf::getYaw(tqtr);
}

double get_yaw(tf::StampedTransform & transform) {
	tf::Quaternion tqtr(transform.getRotation().x(), transform.getRotation().y(),
			transform.getRotation().z(), transform.getRotation().w());
	return tf::getYaw(tqtr);
}

double wave_front_marcher(VECTOR &v, double F) {
	double Ta = INF, Tc = INF;
	int sz = v.size();

	for (int i = 0; i < sz; i++)
		Ta = std::min(v[i], Ta);

	for (int i = 0; i < sz; i++)
		if (v[i] != Ta)
			Tc = std::min(v[i], Tc);

	assert(Ta != INF);

	if (Tc == INF) {
		return Ta + F;
	} else {
		double t = 2 * F * F - ((Ta - Tc) * (Ta - Tc));
		if (t >= 0) {
			t = sqrt(t);
			return (Ta + Tc + t) / 2.0;
		}
		return Ta + F;
	}
}

PII index(double x, double y, double grain) {
	int ix = floor(y / grain);
	int iy = floor(x / grain);

	return PII(ix, iy);
}

DD unindex(int x, int y, double grain) {
	double xx = y * grain;
	double yy = x * grain;
	return DD(xx, yy);
}

double piminuspiAngle(double theta) {
	double mintheta = std::min(theta, 2 * PI - theta);
	double maxtheta = std::max(theta, 2 * PI + theta);
	return mintheta <= PI && mintheta >= -PI ? mintheta : maxtheta;
}

double normalizeAngle(double z) {
	return atan2(sin(z), cos(z));
}

double angleDiff(double a, double b) {
	double d1, d2;
	a = normalizeAngle(a);
	b = normalizeAngle(b);
	d1 = a - b;
	d2 = 2 * PI - fabs(d1);
	if (d1 > 0)
		d2 *= -1.0;
	return fabs(d1) < fabs(d2) ? d1 : d2;
}
